import math
import numpy as np
from scipy.spatial.transform import Rotation
'''
    p=Ptz()
    v=np.array([[0],[0],[1]])
    print (p.back2origin(v,-45))
    
    result:
    root== [0, 0.7071067811865476, 0.7071067811865476]
    theta [0, 0, -45]
    r== [0.         0.70710678 0.70710678]
    [0.         1.41421356 1.41421356]
    
'''
class Ptz:
    def __init__(self):
        self.r=1

# 旋转的时候，坐标轴逆时针，相当于向量顺时针，右手坐标为负数
    
    def euler_mat(self,alpha):
        theta=[0,0,alpha]
        print ("theta",theta)
        r = Rotation.from_euler('zyx', theta, degrees=True)
        # r = Rotation.from_euler('x',alpha, degrees=True)
        return r.as_matrix()

    # 右手坐标,逆时针为正角度
    def origin(self,alpha):
        radians=math.radians(alpha)
        z=self.r*math.cos(radians)
        x=0
        y=-self.r*math.sin(radians)
        return [x,y,z]
    def back2origin(self,v,alpha):
        t=self.origin(alpha)
        print ("root==",t)

        m=self.euler_mat(alpha)
        r=np.matmul(m,v)
        r=np.squeeze(r)
        print ("r==",r)
        v2=r+t
        return v2
        
if __name__ == '__main__':

    p=Ptz()
    v=np.array([[0],[1],[0]])
    print (p.back2origin(v,-45))
    